// #include "opencv2/myopencv.hpp"
// using namespace std;
// 
// double lb = 0, ub = 32767;
// 
// class TRectInfo {
// public:
// 	CvPoint lu;
// 	CvPoint rb;
// 	double depthValue;
// 	CvRect rect;
// 	bool pressed;
// 	CvMat *canvas;
// 	string name;
// 
// 	TRectInfo(void) {
// 		pressed = false;
// 		canvas = NULL;
// 		depthValue = 0;
// 	}
// 	~TRectInfo(void) {
// 		if (canvas != NULL)
// 			cvReleaseMat(&canvas);
// 	}
// };
// TRectInfo videoRect, depthRect;
// 
// class DepthPlayer{
// public:
// 	FILE* fin;
// 	long long startFilePos;
// 	long long fileSize;
// 	int w, h;
// 	int frameNum;
// 	int curFrame;
// 	int frameSize;
// 	int seconds;
// 	short* frame;
// 	CvMat* mat;
// 	CvMat* newmat;
// 	CvMat* backup;
// 	bool isSkipTo;
// 	int restart;
// 	bool pause;
// 	DepthPlayer() {
// 		fin = NULL;
// 		startFilePos = fileSize = 0;
// 		w = h = 0;
// 		frameNum = curFrame = frameSize = 0;
// 		seconds = 0;
// 		frame = NULL;
// 		mat = newmat = backup = NULL;
// 		isSkipTo = true;
// 		restart = 0;
// 		pause = false;
// 	}
// 
// 	void initParamter(string name) {
// 		fopen_s(&fin, name.c_str(), "rb");
// 		fscanf_s(fin, "%d %d\n", &w, &h);
// 		startFilePos = _ftelli64(fin);
// 		_fseeki64(fin, 0, SEEK_END);
// 		fileSize = _ftelli64(fin);
// 		_fseeki64(fin, startFilePos, SEEK_SET);
// 
// 		frameSize = w*h*sizeof(short);
// 		frameNum = int((fileSize-startFilePos) / frameSize);
// 		seconds = frameNum / 30;
// 
// 		frame = new short[w*h];
// 		mat = cvCreateMatHeader(h, w, CV_16SC1);
// 		newmat = cvCreateMat(h, w, CV_64FC1);
// 		backup = cvCreateMat(h, w, CV_16SC1);
// 		cvZero(backup);
// 	}
// 
// 	void myflip( const uchar* src, size_t sstep, uchar* dst, size_t dstep, cv::Size size, size_t esz )
// 	{
// 		int i, j, limit = (int)(((size.width + 1)/2)*esz);
// 		cv::AutoBuffer<int> _tab(size.width*esz);
// 		int* tab = _tab;
// 
// 		for( i = 0; i < size.width; i++ )
// 			for( size_t k = 0; k < esz; k++ )
// 				tab[i*esz + k] = (int)((size.width - i - 1)*esz + k);
// 
// 		for( ; size.height--; src += sstep, dst += dstep )
// 		{
// 			for( i = 0; i < limit; i++ )
// 			{
// 				j = tab[i];
// 				uchar t0 = src[i], t1 = src[j];
// 				dst[i] = t1; dst[j] = t0;
// 			}
// 		}
// 	}
// 
// 	void updateMat(void) {
// 		mat->data.s = frame;
// 		cv::Mat src = cv::cvarrToMat(mat);
// 		size_t esz = src.elemSize();
// 		myflip( src.data, src.step, src.data, src.step, src.size(), esz );
// 		// 		cvSmooth(mat, mat);
// 		// 		cvFlip(mat, NULL, 1);
// 	}
// 
// 	void recovery(void)
// 	{
// 		short* matPtr;
// 		short* backupPtr;
// 		matPtr = mat->data.s;
// 		backupPtr = backup->data.s;
// 		for (int i=0; i<mat->rows; ++i)
// 			for (int j=0; j<mat->cols; ++j, ++matPtr, ++backupPtr) {
// 				if (*matPtr < 0) *matPtr = *backupPtr;
// 				if (*matPtr<lb || *matPtr>ub) *matPtr = 0;
// 			}
// 	}
// 
// 	~DepthPlayer() {
// 		fclose(fin);
// 		delete[] frame;
// 		cvReleaseMat(&mat);
// 		cvReleaseMat(&newmat);
// 		cvReleaseMat(&backup);
// 	}
// };
// 
// DepthPlayer *curPlayer = NULL;
// 
// inline int cvFitIn(int x, int a, int b) {return min(max(a,x),b);}
// 
// void moveRect(int type, int x, int y, int flag, void* param) {
// 	TRectInfo *rectInfo = (TRectInfo*)param;
// 	CvRect rect = rectInfo->rect;
// 	CvPoint first, second;
// 
// 	switch (type) {
// 	case CV_EVENT_LBUTTONDOWN:
// 		rectInfo->pressed = true;
// 		rectInfo->lu.x = x;
// 		rectInfo->lu.y = y;
// 		break;
// 	case CV_EVENT_LBUTTONUP:
// 		{
// 			CvMat *canvas = cvCloneMat(rectInfo->canvas);
// 
// 			rect.x = cvFitIn(rect.x - (rectInfo->lu.x - x), 0, canvas->cols-rect.width);
// 			rect.y = cvFitIn(rect.y - (rectInfo->lu.y - y), 0, canvas->rows-rect.height);
// 
// 			CvFont font;
// 			cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.7, 0.7);
// 			char words[100];
// 			sprintf_s(words, 100, "(x,y)=(%d, %d)", rect.x, rect.y);
// 			int maxInt = (1 << CV_ELEM_SIZE(canvas->type)*8) - 1;
// 			cvPutText(canvas, words, cvPoint(50,50), &font, cvScalar(maxInt));
// 			
// 			first.x = rect.x;
// 			first.y = rect.y;
// 			second.x = first.x + rect.width;
// 			second.y = first.y + rect.height;
// 
// 			cvRectangle(canvas, first, second, cvScalar(maxInt));
// 			cvShowImage(rectInfo->name.c_str(), canvas);
// 			cvReleaseMat(&canvas);
// 			rectInfo->rect = rect;
// 		}
// 		break;
// 	case CV_EVENT_MOUSEMOVE:
// 		if (flag & CV_EVENT_FLAG_LBUTTON) {
// 			CvMat *canvas = cvCloneMat(rectInfo->canvas);
// 
// 			rect.x = cvFitIn(rect.x - (rectInfo->lu.x - x), 0, canvas->cols-rect.width);
// 			rect.y = cvFitIn(rect.y - (rectInfo->lu.y - y), 0, canvas->rows-rect.height);
// 
// 			CvFont font;
// 			cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.7, 0.7);
// 			char words[100];
// 			sprintf_s(words, 100, "(x,y)=(%d, %d)", rect.x, rect.y);
// 			int maxInt = (1 << CV_ELEM_SIZE(canvas->type)*8) - 1;
// 			cvPutText(canvas, words, cvPoint(50,50), &font, cvScalar(maxInt));
// 
// 			first.x = rect.x;
// 			first.y = rect.y;
// 			second.x = first.x + rect.width;
// 			second.y = first.y + rect.height;
// 
// 			cvRectangle(canvas, first, second, cvScalar(maxInt/2));
// 			cvShowImage(rectInfo->name.c_str(), canvas);
// 			cvReleaseMat(&canvas);
// 		}
// 		break;
// 	default:
// 		break;
// 	}
// }
// 
// 
// bool myGt(const pair<int, double> &x, const pair<int, double> &y) {
// 	return x.first > y.first;
// }
// class SimpleHistogram : public CvHistogram {
// public:
// 	int *bins;
// 	int binNum;
// 	double binW;
// 	double ranges[2];
// 	CvMat *colors;
// 	double *eachBinRg;
// 	pair<int, double> *pairs;
// 
// public:
// 
// 	SimpleHistogram(void) {
// 		bins = NULL;
// 		colors = NULL;
// 		eachBinRg = NULL;
// 		pairs = NULL;
// 	}
// 	~SimpleHistogram(void) {
// 		if (pairs != NULL) delete[] pairs;
// 		if (eachBinRg != NULL) delete[] eachBinRg;
// 		if (bins != NULL) delete[] bins;
// 		if (colors != NULL) cvReleaseMat(&colors);
// 	}
// 
// 	void setRange(double a, double b) {
// 		ranges[0] = a;
// 		ranges[1] = b;
// 		if (bins != NULL)
// 			binW = (b - a) / binNum;
// 	}
// 	void setBinNum(int n) {
// 		binNum = n;
// 		if (pairs != NULL) {
// 			delete[] pairs;
// 			pairs = NULL;
// 		}
// 		if (eachBinRg != NULL) {
// 			delete[] eachBinRg;
// 			eachBinRg = NULL;
// 		}
// 		if (bins != NULL) delete[] bins;
// 		bins = new int[n];
// 		memset(bins, 0, sizeof(int)*n);
// 		binW = (ranges[1]-ranges[0]) / binNum;
// 	}
// 
// 	void sortBins(void) {
// 		if (pairs == NULL)
// 			pairs = new pair<int, double>[binNum];
// 		for (int i=0; i<binNum; ++i) {
// 			pairs[i].first = bins[i];
// 			pairs[i].second = i*binW;
// 		}
// 		sort(pairs, pairs+binNum, myGt);
// 		if (eachBinRg == NULL)
// 			eachBinRg = new double[binNum];
// 		for (int i=0; i<binNum; ++i) {
// 			bins[i] = pairs[i].first;
// 			eachBinRg[i] = pairs[i].second;
// 		}
// 	}
// 	void accumulate(void) {
// 		for (int i=1; i<binNum; ++i)
// 			bins[i] += bins[i-1];
// 	}
// 
// 	template<typename T>
// 	void calcHistFast(CvMat *mat) {
// 		memset(bins, 0, sizeof(int)*binNum);
// 		T *ptr;
// 		int idx;
// 		for (int i=0; i<mat->rows; ++i) {
// 			ptr = (T*)cvPtr2D(mat, i, 0);
// 			for (int j=0; j<mat->cols; ++j, ++ptr) {
// 				idx = cvFloor((*ptr-ranges[0])/binW);
// 				if (0 <= idx && idx < binNum)
// 					++bins[idx];
// 			}
// 		}
// 	}
// 
// 	void threshHist(double atLeast) {
// 		for (int i=0; i<binNum; ++i)
// 			if (bins[i] < atLeast) bins[i] = 0;
// 	}
// 
// 	void createColor(void) {
// 		if (colors != NULL)
// 			cvReleaseMat(&colors);
// 		colors = cvCreateMat(1, binNum, CV_8UC3);
// 		for( int i = 0; i < binNum; i++ ) {
// 			uchar *ptr = cvPtr2D(colors, 0, i);
// 			ptr[0] = uchar(i*180./binNum);
// 			ptr[1] = 255;
// 			ptr[2] = 255;
// 		}
// 		cvCvtColor(colors, colors, CV_HSV2BGR);
// 	}
// 
// 	void show(char *winName=NULL, int binW=20, int firstN = -1) {
// 		if (winName == NULL) winName = "histogram";
// 		if (binW <= 0) binW = 20;
// 		if (firstN == -1) firstN = binNum;
// 
// 		CvMat *histImg = cvCreateMat(200, binW*firstN, CV_8UC3);
// 
// 		int maxBinVal = numeric_limits<int>::min();
// 		for (int i=0; i<firstN; ++i)
// 			if (bins[i] > maxBinVal) maxBinVal = bins[i];
// 
// 		if (maxBinVal == 0) maxBinVal = 200;
// 
// 		if (colors == NULL || firstN >= colors->cols) createColor();
// 
// 		CvFont font;
// 		cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.4, 0.4);
// 		char words[100];
// 
// 		for( int i = 0; i < firstN; i++ ) {
// 			int val = bins[i] * histImg->rows /maxBinVal;
// 			cvRectangle( histImg, cvPoint(i*binW, histImg->rows),
// 				cvPoint((i+1)*binW, histImg->rows - val),
// 				cvGet2D(colors, 0, i), -1, 8 );
// 			sprintf_s(words, 100, "%d", i);
// 			if (binW > 18)
// 				cvPutText(histImg, words, cvPoint(i*binW, histImg->rows-20), &font, cvScalar(255,255,255));
// 		}
// 		sprintf_s(words, "maxV: %d, val/pixel: %f.", maxBinVal, float(maxBinVal)/histImg->rows);
// 		cvPutText(histImg, words, cvPoint(20,10), &font, cvScalar(255,255,255));
// 		cvShowImage(winName, histImg);
// 
// 		cvReleaseMat(&histImg);
// 	}
// 
// 	double mainMean(double perc) {
// 		int *bakBins = new int[binNum];
// 		memcpy(bakBins, bins, binNum*sizeof(int));
// 		sortBins();
// 		accumulate();
// 
// 		double mean = 0;
// 		int cnt = 0;
// 		for (int i=0; i<binNum; ++i)
// 			if (bins[i] <= perc * bins[binNum-1]) {
// 				mean += bins[i] * (eachBinRg[i]+binW/2);
// 				cnt += bins[i];
// 			}
// 			else {
// 				mean /= cnt;
// 				break;
// 			}
// 
// 			memcpy(bins, bakBins, binNum*sizeof(int));
// 			delete[] bakBins;
// 			return mean;
// 	}
// 
// 	void derive(void) {
// 		for (int i=0; i<binNum-1; ++i) {
// 			bins[i] = abs(bins[i+1] - bins[i]);
// 		}
// 		bins[binNum-1] = 0;
// 	}
// 
// };
// 
// void bothPlay(string depthName, string videoName) {
// 	FILE* outFile;
// 	//video x,y   depth x,y,z
// 	outFile = fopen("anno.txt", "w");
// 
// 	curPlayer = new DepthPlayer;
// 	curPlayer->initParamter(depthName);
// 	cvNamedWindow("video tracking");
// 	cvMoveWindow("video tracking", 20, 0);
// 	cvSetMouseCallback("video tracking", moveRect, &videoRect);
// 
// 	cvNamedWindow("depth tracking");
// 	cvMoveWindow("depth tracking", 700, 0);
// 	cvSetMouseCallback("depth tracking", moveRect, &depthRect);
// 
// 	cv::VideoCapture video;
// 	video.open(videoName);
// 
// 	curPlayer->curFrame = 0;
// 	int readFrames = 0;
// 	long long startTime;
// 	double cpufreq = cvGetTickFrequency();
// 
// 	cv::Mat videoMat;
// 	cv::Mat backupVideoMat;// = cv::Mat::zeros(curPlayer->h, curPlayer->w, CV_16SC1);
// 	CvMat videoMatCanvas;
// 
// 
// 	int videoStartFrame = 35;
// 	for (int i=0; i<videoStartFrame; ++i)
// 		video.read(videoMat);
// 
// 	CvMat *depthRecovMat = cvCloneMat(curPlayer->backup);
// 	cv::Mat videoOriginMat16(curPlayer->h, curPlayer->w, CV_16SC1);
// 
// 	bool firstShow = true;
// 
// 	while (true) {
// 		if (readFrames % 10 == 0) {
// 			startTime = cvGetTickCount();	
// 		}
// 		fread_s(curPlayer->frame, curPlayer->frameSize, sizeof(short), 
// 			curPlayer->w*curPlayer->h, curPlayer->fin);
// 
// 		video.read(videoMat);
// 		cv::cvtColor(videoMat, videoMat, cv::COLOR_RGB2GRAY);
// 		videoMat.copyTo(backupVideoMat);
// 
// 		++readFrames;
// 		if (readFrames % 10 == 0) {
// 			cout << "FPS: " << 10 / ((cvGetTickCount() - startTime) / cpufreq / 1000000) << endl;
// 			video.read(videoMat);
// 			cv::cvtColor(videoMat, videoMat, cv::COLOR_RGB2GRAY);
// 		}
// 		if (readFrames % 100 == 0) {
// 			video.read(videoMat);
// 			cv::cvtColor(videoMat, videoMat, cv::COLOR_RGB2GRAY);
// 		}
// 
// 		curPlayer->updateMat();
// 		curPlayer->recovery();
// 
// 		++curPlayer->curFrame;
// 
// 		if (curPlayer->curFrame % 20 != 1) continue;
// 
// 		cv::Mat equalMat;
// 
// // 		imshow("before equalize", videoMat);
// 		cv::equalizeHist(videoMat, equalMat);
// // 		imshow("after equalize", equalMat);
// 
// 		if (firstShow) {
// 			char c=27;
// 			firstShow = false;
// 			cvNamedWindow("first video");
// 			cvNamedWindow("first depth");
// 
// 			while (c==27 || !depthRect.pressed || !videoRect.pressed) {
// 				if (!depthRect.pressed)
// 					cvShowImage("first depth", curPlayer->mat);
// 				if (!videoRect.pressed)
// 					imshow("first video", videoMat);
// 
// 				depthRect.canvas = cvCloneMat(curPlayer->mat);
// 				videoMatCanvas = videoMat;
// 				videoRect.canvas = cvCloneMat(&videoMatCanvas);
// 
// 				videoRect.name = "first video";
// 				depthRect.name = "first depth";
// 
// // 				c = cvWaitKey(0);
// 				videoRect.lu.x = 158;
// 				videoRect.lu.y = 190;
// 				videoRect.rb.x = 435;
// 				videoRect.rb.y = 467;
// 				depthRect.lu.x = 301;
// 				depthRect.lu.y = 217;
// 				depthRect.rb.x = 429;
// 				depthRect.rb.y = 345;
// 				depthRect.pressed = true;
// 				videoRect.pressed = true;
// 				c=0;
// 
// 				cout << "char: " << (int)c << endl;
// 			}
// 			cvDestroyWindow("first depth");
// 			cvDestroyWindow("first video");
// 			CvRect rect;
// 			rect.x = videoRect.lu.x;
// 			rect.y = videoRect.lu.y;
// 			rect.width = videoRect.rb.x - rect.x;
// 			rect.height = videoRect.rb.y - rect.y;
// 			videoRect.rect = rect;
// 
// 			rect.x = depthRect.lu.x;
// 			rect.y = depthRect.lu.y;
// 			rect.width = depthRect.rb.x - rect.x;
// 			rect.height = depthRect.rb.y - rect.y;
// 			depthRect.rect = rect;
// 
// 
// 			cvShowImage("depth tracking", curPlayer->mat);
// 			imshow("video tracking", videoMat);
// 			depthRect.name = "depth tracking";
// 			videoRect.name = "video tracking";
// 		}
// 		else {
// 			char c = 0;
// 			while (c != 13) {
// 				{
// 					CvPoint first, second;
// 					first.x = depthRect.rect.x;
// 					first.y = depthRect.rect.y;
// 					second.x = first.x + depthRect.rect.width;
// 					second.y = first.y + depthRect.rect.height;
// 					cvCopy(curPlayer->mat, depthRect.canvas);
// 					cvRectangle(depthRect.canvas, first, second, cvScalar(32767));
// 				}
// 				{
// 					cv::Point first, second;
// 					first.x = cvFitIn(videoRect.rect.x, 0, videoMat.cols);
// 					first.y = cvFitIn(videoRect.rect.y, 0, videoMat.rows);
// 					second.x = cvFitIn(first.x + videoRect.rect.width, 0, videoMat.cols);
// 					second.y = cvFitIn(first.y + videoRect.rect.height, 0, videoMat.rows);
// 					videoMatCanvas = equalMat;
// 					cvCopy(&videoMatCanvas, videoRect.canvas);
// 					cvRectangle(videoRect.canvas, first, second, cvScalar(32767));
// 				}
// 				cvShowImage("depth tracking", depthRect.canvas);
// 				cvShowImage("video tracking", videoRect.canvas);
// 
// 				c = cvWaitKey(0);
// 				cout << "pressed: " << int(c) << endl;
// 				if (c == 27) {
// 					fclose(outFile);
// 					exit(0);
// 				}
// 				if (c == 32) {
// 					break;
// 				}
// 			}
// 			if (c != 32) {
// 				int ub2 = 32767, lb2 = 10, rgPerBin = 4;
// 				SimpleHistogram myHist;
// 				CvMat *subMat;
// 				myHist.setBinNum(cvCeil((ub2-lb2)/rgPerBin));
// 				myHist.setRange(lb2, ub2);
// 				
// 				subMat = cvCreateMatHeader(depthRect.rect.height, depthRect.rect.width, 
// 					curPlayer->mat->type);
// 				cvGetSubRect(curPlayer->mat, subMat, depthRect.rect);
// 				myHist.calcHistFast<short>(subMat);
// 				depthRect.depthValue = myHist.mainMean(0.8);
// 				cvReleaseMatHeader(&subMat);
// 
// 				printf("%d %d %lf\n", depthRect.rect.x, depthRect.rect.y, depthRect.depthValue);
// 
// 				fprintf(outFile, "%d %d %lf \t %d %d\n", 
// 					depthRect.rect.x, depthRect.rect.y, depthRect.depthValue, 
// 					videoRect.rect.x, videoRect.rect.y);
// // 				fflush(outFile);
// 			}
// 		}
// 
// 	}
// 	cvReleaseMat(&depthRecovMat);
// }
// 
// int main2(int argc, char* argv[])
// {
// 	string depthName, videoName;
// 	depthName = string(argv[1]);
// 	videoName = string(argv[2]);
// 	if (depthName[depthName.length()-1] == 'i')
// 		swap(depthName, videoName);
// 
// 	cout << "depth file: " << depthName << endl << "video file: " << videoName << endl;
// 
// 	bothPlay(depthName, videoName);
// 	return 0;
// }